#include "rosclient.h"
#include <qdebug.h>



RosClient::RosClient(string remoteServerAddressIn)
{

//    tmpUserInfo=tmpUserInfoIn;
    //qDebug()<<"init RosCommunicate";
    soap_init(&client_soap);
    client_soap.connect_timeout = 3;
    client_soap.send_timeout = 3;//超时判定
    client_soap.recv_timeout = 3;
    soap_set_mode(&client_soap,SOAP_C_UTFSTRING);//UTF-8


    remoteServerAddress=remoteServerAddressIn;
//    remoteServerAddress=remoteServerAddressIn;
//    remoteServerAddress ="127.0.0.1:5080";
//    isInitialStatus=false;

    //---------初始化MC线程锁--------
    pthread_mutexattr_init(&mutexAttr);
    pthread_mutexattr_setprotocol(&mutexAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&communicateMutex,&mutexAttr);

    isInitialStatus=true;



}

int RosClient::changeRemoteServerAddress(string remoteServerAddressIn)
{
    pthread_mutex_lock(&communicateMutex);
    remoteServerAddress=remoteServerAddressIn;
    pthread_mutex_unlock(&communicateMutex);
    return 1;
}


int RosClient::setMapSolver(rosgsoapnamespace::E_MAP_SOLVER solverIn, int laserCount, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setMapSolver(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,solverIn,laserCount,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setMapSolver = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::createMapStart(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_createMapStart(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::createMapStart = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::createMapEnd(std::string mapName, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_createMapEnd(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,mapName,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::createMapEnd = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::loadMap(std::string mapName, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_loadMap(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,mapName,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::loadMap = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::addVitrualBlock(std::string configFilePath, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_addVitrualBlock(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,configFilePath,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::addVitrualBlock = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::deleteVitrualBlock(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_deleteVitrualBlock(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::deleteVitrualBlock = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::addForbidenZone(double x_start, double y_start, double x_end, double y_end, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_addForbidenZone(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,x_start,y_start,x_end,y_end,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::addForbidenZone = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setOdomSolver(rosgsoapnamespace::E_ODOM_SOLVER solverIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setOdomSolver(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,solverIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setOdomSolver = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setLocationSolver(rosgsoapnamespace::E_MAP_SOLVER solverIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setLocationSolver(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,solverIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setLocationSolver = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getVehiclePosition(hmigsoapnamespace::VehiclePosition& positionReturn)
{
    int communicateFlag=-1;
    rosgsoapnamespace::VehiclePosition tmpPositionReturn;
    communicateFlag=getVehiclePosition(tmpPositionReturn);
    positionReturn=fromRosData(tmpPositionReturn);
    return communicateFlag;
}

int RosClient::getVehiclePosition(rosgsoapnamespace::VehiclePosition &positionReturn)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_getVehiclePosition(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,positionReturn) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getVehiclePosition = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}
int RosClient::setVehiclePosition(hmigsoapnamespace::VehiclePosition positionIn,int &returnFlag)
{
    rosgsoapnamespace::VehiclePosition tmpPositionReturn=fromHmiData(positionIn);
    return setVehiclePosition(tmpPositionReturn,returnFlag);

}

int RosClient::setVehiclePosition(rosgsoapnamespace::VehiclePosition positionIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setVehiclePosition(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,positionIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setVehiclePosition = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setReplanCoutLimitWhenFailure(int countLimit, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setReplanCoutLimitWhenFailure(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,countLimit,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setReplanCoutLimitWhenFailure = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setTrajectoryGlobalPlanSolver(rosgsoapnamespace::E_TRAJECTORY_GLOBAL_SOLVER solverIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setTrajectoryGlobalPlanSolver(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,solverIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setTrajectoryGlobalPlanSolver = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setTrajectoryLocalPlanSolver(rosgsoapnamespace::E_TRAJECTORY_LOCAL_SOLVER solverIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setTrajectoryLocalPlanSolver(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,solverIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setTrajectoryLocalPlanSolver = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::moveToPosition(hmigsoapnamespace::VehiclePosition positionIn,
                   double velocity,int &returnFlag)
{

    rosgsoapnamespace::VehiclePosition tmpPosition=fromHmiData(positionIn);
    return moveToPosition(tmpPosition,velocity,returnFlag);
}

int RosClient::moveToPosition(rosgsoapnamespace::VehiclePosition positionIn, double velocity, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_moveToPosition(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,positionIn,velocity,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::moveToPosition = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}
int RosClient::moveFollowPath(std::vector<hmigsoapnamespace::VehiclePosition> pathIn,
                   double velocity,int &returnFlag)
{
    std::vector<rosgsoapnamespace::VehiclePosition> tmpPath=fromHmiData(pathIn);
    return moveFollowPath(tmpPath,velocity,returnFlag);
}

int RosClient::moveFollowPath(std::vector<rosgsoapnamespace::VehiclePosition> pathIn, double velocity, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_moveFollowPath(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,pathIn,velocity,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::moveFollowPath = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::changeVelocity(double velocity, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_changeVelocity(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,velocity,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::changeVelocity = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getMovingStatus(rosgsoapnamespace::E_GOAL_STATUS &status)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_getMovingStatus(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,status) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getMovingStatus = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getRemainningTime(double &timeOut)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_getRemainningTime(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,timeOut) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getRemainningTime = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getRemainningDistance(double &distanceOut)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_getRemainningDistance(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,distanceOut) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getRemainningDistance = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::ceaseMotion(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_ceaseMotion(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::ceaseMotion = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::startMotion(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_startMotion(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::startMotion = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::cancelMotion(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_cancelMotion(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::cancelMotion = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setPositionArrivedAccuracy(double accuracyIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setPositionArrivedAccuracy(&client_soap,remoteServerAddress.c_str(),""
                                    ,tmpUserInfo ,accuracyIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setPositionArrivedAccuracy = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setLocalPlanRange(double rangeIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setLocalPlanRange(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,rangeIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setLocalPlanRange = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setVehicleBorderOffset(std::vector<double> point_x, std::vector<double> point_y, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setVehicleBorderOffset(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,point_x,point_y,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setVehicleBorderOffset = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setGlobalBarrierOffset(double offsetIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setGlobalBarrierOffset(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,offsetIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setGlobalBarrierOffset = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setLocalBarrierOffset(double offsetIn, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setLocalBarrierOffset(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,offsetIn,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setLocalBarrierOffset = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::resetRosAll(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_resetRosAll(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::resetRosAll = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getRosPackageStatus(rosgsoapnamespace::E_ROS_PACKAGE package, rosgsoapnamespace::E_ROS_PACKAGE_STATUS &status)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;

    if(soap_call_getRosPackageStatus(&client_soap,remoteServerAddress.c_str(),""
                                     ,tmpUserInfo,package,status) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getRosPackageStatus = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::startImuCalibrate(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_startImuCalibrate(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::startImuCalibrate = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::stopImuCalibrate(int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_stopImuCalibrate(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::stopImuCalibrate = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getMessage_hmi(std::vector<hmigsoapnamespace::Message> &messageListReturn)
{
    int communicateFlag=-1;
    std::vector<rosgsoapnamespace::Message> tmpMessage;
    communicateFlag=getMessage_ros(tmpMessage);
    messageListReturn=fromRosData(tmpMessage);
    return communicateFlag;
}

int RosClient::getMessage_ros(std::vector<rosgsoapnamespace::Message> &messageListReturn)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    int tmpKey=soap_call_getMessage(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,messageListReturn) ;
    if(tmpKey== SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getMessage = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}


int RosClient::setWheelOdometerData_hmi(int robotIdIn, hmigsoapnamespace::OdemeterData odemeterData, int &returnFlag)
{
    rosgsoapnamespace::OdemeterData tmpData=fromHmiData(odemeterData);
    return setWheelOdometerData_ros(robotIdIn,tmpData,returnFlag);

}

int RosClient::setWheelOdometerData_ros(int robotIdIn, rosgsoapnamespace::OdemeterData odemeterData, int &returnFlag)
{

    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setWheelOdometerData(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,robotIdIn,odemeterData,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setWheelOdometerData = ok";

    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::getVehicleVelocityComand_hmi(int robotIdIn,hmigsoapnamespace::VehicleVelocity &velocityComamnd)
{
    rosgsoapnamespace::VehicleVelocity tmpVelocity;
    int tmpFlag=getVehicleVelocityComand_ros(robotIdIn,tmpVelocity);
    velocityComamnd=fromRosData(tmpVelocity);
    return tmpFlag;
}

int RosClient::getVehicleVelocityComand_ros(int robotIdIn, rosgsoapnamespace::VehicleVelocity &velocityComamnd)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_getVehicleVelocityComand(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,robotIdIn,velocityComamnd) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::getVehicleVelocityComand = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setWheelMoveSpeed(int robotIdIn, std::vector<double> wheelMoveSpeed, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setWheelMoveSpeed(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,robotIdIn,wheelMoveSpeed,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setWheelMoveSpeed = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

int RosClient::setWheelPathLength(int robotIdIn, std::vector<double> wheelPathLength, int &returnFlag)
{
    pthread_mutex_lock(&communicateMutex);
    int communicateFlag=-1;
    if(soap_call_setWheelPathLength(&client_soap,remoteServerAddress.c_str(),"",tmpUserInfo,robotIdIn,wheelPathLength,returnFlag) == SOAP_OK)
    {
        communicateFlag=0;
        qDebug()<<"RosClient::setWheelPathLength = ok";
    }
    else
    {
        //qDebug("soap error:%d,%s,%s", client_soap.error, *soap_faultcode(&client_soap), *soap_faultstring(&client_soap) );
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    pthread_mutex_unlock(&communicateMutex);
    return communicateFlag;
}

std::vector<rosgsoapnamespace::VehiclePosition> RosClient::fromHmiData
                                        (std::vector<hmigsoapnamespace::VehiclePosition> dataIn)
{
    std::vector<rosgsoapnamespace::VehiclePosition> tmpData;
    tmpData.resize(dataIn.size());
    for(int i=0;i<dataIn.size();i++)
    {
        tmpData[i].x=dataIn[i].x;
        tmpData[i].y=dataIn[i].y;
        tmpData[i].rotate=dataIn[i].rotate;
    }
    return tmpData;

}

rosgsoapnamespace::VehiclePosition RosClient::fromHmiData(hmigsoapnamespace::VehiclePosition dataIn)
{
    rosgsoapnamespace::VehiclePosition tmpData;

    tmpData.x=dataIn.x;
    tmpData.y=dataIn.y;
    tmpData.rotate=dataIn.rotate;

    return tmpData;
}

rosgsoapnamespace::VehicleVelocity RosClient::fromHmiData(hmigsoapnamespace::VehicleVelocity dataIn)
{
    rosgsoapnamespace::VehicleVelocity tmpData;

    tmpData.x_move=dataIn.x_move;
    tmpData.y_move=dataIn.y_move;
    tmpData.z_rotate=dataIn.z_rotate;

    return tmpData;
}




rosgsoapnamespace::OdemeterData RosClient::fromHmiData(hmigsoapnamespace::OdemeterData dataIn)
{
    rosgsoapnamespace::OdemeterData tmpData;

    tmpData.vel=fromHmiData(dataIn.vel);
    tmpData.pos=fromHmiData(dataIn.pos);

    return tmpData;
}

 hmigsoapnamespace::VehiclePosition RosClient::fromRosData(rosgsoapnamespace::VehiclePosition dataIn)
 {
     hmigsoapnamespace::VehiclePosition tmpData;

     tmpData.x=dataIn.x;
     tmpData.y=dataIn.y;
     tmpData.rotate=dataIn.rotate;

     return tmpData;
 }

 hmigsoapnamespace::VehicleVelocity RosClient::fromRosData(rosgsoapnamespace::VehicleVelocity dataIn)
 {
     hmigsoapnamespace::VehicleVelocity tmpData;

     tmpData.x_move=dataIn.x_move;
     tmpData.y_move=dataIn.y_move;
     tmpData.z_rotate=dataIn.z_rotate;

     return tmpData;
 }

std::vector<hmigsoapnamespace::Message>  RosClient::fromRosData(std::vector<rosgsoapnamespace::Message> dataIn)
{
    std::vector<hmigsoapnamespace::Message> tmpData;
    tmpData.resize(dataIn.size());
    for(int i=0;i<dataIn.size();i++)
    {
        tmpData[i].MessageLevel=dataIn[i].MessageLevel;
        tmpData[i].time=dataIn[i].time;
        tmpData[i].robotId=dataIn[i].robotId;
        tmpData[i].componentClassName=dataIn[i].componentClassName;
        tmpData[i].messageType=dataIn[i].messageType;
        tmpData[i].messageCode=dataIn[i].messageCode;
        tmpData[i].parameter1=dataIn[i].parameter1;
        tmpData[i].parameter2=dataIn[i].parameter2;
        tmpData[i].parameter3=dataIn[i].parameter3;
        tmpData[i].parameter4=dataIn[i].parameter4;
        tmpData[i].MessageInformation=dataIn[i].MessageInformation;
    }
    return tmpData;
}


